/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <GL/glut.h>
#include "GLDisplay.hpp"
#include "Machine.hpp"
#include "picking/glutils.hpp"
#include "MuRoMainFrame.h"

using namespace MuRo;

BEGIN_EVENT_TABLE(CGLDisplay,wxGLCanvas)
  EVT_PAINT(CGLDisplay::OnPaint)
  EVT_SIZE(CGLDisplay::OnSize)
  EVT_ERASE_BACKGROUND(CGLDisplay::OnErase)
  
  EVT_LEFT_DOWN(CGLDisplay::OnLeftDown)
  EVT_LEFT_UP(CGLDisplay::OnLeftUp)
  EVT_LEFT_DCLICK(CGLDisplay::OnDoubleClick)
  EVT_RIGHT_DOWN(CGLDisplay::OnRightDown)
  EVT_RIGHT_UP(CGLDisplay::OnRightUp)
  EVT_MOTION(CGLDisplay::OnMotion)
  EVT_MOUSEWHEEL(CGLDisplay::OnMouseWheel)
  EVT_LEAVE_WINDOW(CGLDisplay::OnLeaveWindow)
    
  EVT_KEY_DOWN(CGLDisplay::OnKeyDown)
  EVT_KEY_UP(CGLDisplay::OnKeyUp)
  EVT_CHAR(CGLDisplay::OnChar)
  //EVT_CONTEXT_MENU(CGLDisplay::OnContextMenu)
END_EVENT_TABLE()

int CGLDisplay::attribList[3]={WX_GL_DOUBLEBUFFER,WX_GL_RGBA,0};



CGLDisplay::CGLDisplay(wxWindow* parent, MuRoMainFrame* mainFrame)
:wxGLCanvas(parent, wxID_ANY, attribList),
mainFrame(mainFrame),
GLContext(this),
followSelected(false),
//operationMode(KOperModeManualDrive),
grid(true),
gridZ(-1.0),
gridSize(1.0),
// scale2d(100),
// viewOriginX(0),
// viewOriginY(0),
mouseClickX(0), 
mouseClickY(0),
throttle(0.5),
keyUp(false), keyDown(false), keyRight(false), keyLeft(false), keyBoost(false),
driving(false),
posSetHeading(0),
contextMenuOnMachine(false)
{
  
  view.Scale=100;
  view.OriginX=0;
  view.OriginY=0;
  
  SetCallback(this);
  AddPickable(this);
  
  //disable focus picking
  EnableFocusEvents(false);
  
  RunThread();
}

CGLDisplay::~CGLDisplay()
{
  if (IsThreadRunning()) {
    SetRequestTermination();
    WaitThread();
  }
}

void CGLDisplay::SetFollowMode(bool follow)
{
  followSelected=follow;
}

void CGLDisplay::SetGrid(bool grid)
{
  this->grid=grid;
}

/****************** event handlers **************************/


void CGLDisplay::OnErase(wxEraseEvent &e)
{
  /*do nothing*/
}

void CGLDisplay::OnPaint(wxPaintEvent &e)
{
  wxPaintDC dc(this);

  draw();
}

void CGLDisplay::OnSize(wxSizeEvent &e)
{
  view.Width=e.GetSize().GetWidth();
  view.Height=e.GetSize().GetHeight();
}

void CGLDisplay::OnLeftDown(wxMouseEvent &e)
{
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition)
  {
    mainFrame->GetSelectedMachine()->SetPose(posSetX, posSetY, posSetHeading);
    
    // We set back to SetPosition mode, as this will toggle the mode back the previous mode.
    mainFrame->GetSelectedMachine()->SetOperationMode(KOperModeSetPosition);

  } else if (mainFrame->GetSelectedMachine() && 
             mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeCoordDrive &&
             editingPath) {
    float x,y,z;
    screenToGl(e.m_x,e.m_y,0, x, y, z);
    mainFrame->GetSelectedMachine()->AddPathPoint(x,y);
    dPrint(1,"set point %f %f",x,y);
  } else {
    dPrint(1,"left pressed");
    mouseClickX=e.m_x;
    mouseClickY=e.m_y;
    OnMouseDown(e.m_x,e.m_y,1);
  }
}

void CGLDisplay::OnLeftUp(wxMouseEvent &e)
{
  OnMouseUp(e.m_x,e.m_y);
}

void CGLDisplay::OnDoubleClick(wxMouseEvent &e)
{
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeCoordDrive && editingPath)
  {
    editingPath=false;
  } else {
    picked=false;
    dPrint(1,"DoubleClicked");
    OnMouseDown(e.m_x,e.m_y,3); //use number 3 button for double click
    OnMouseUp(e.m_x,e.m_y);
    if (!picked && mainFrame->GetSelectedMachine())
    {
      float x,y,z;
      screenToGl(e.m_x,e.m_y,0, x, y, z);
      mainFrame->GetSelectedMachine()->DriveToPoint(x,y);
    }
  }
}

void CGLDisplay::OnRightDown(wxMouseEvent &e)
{
  contextMenuOnMachine=false;
  OnMouseDown(e.m_x,e.m_y,2);
  dPrint(1,"right down");
  
}

void CGLDisplay::OnRightUp(wxMouseEvent &e)
{
  OnMouseUp(e.m_x,e.m_y);
  if (contextMenuOnMachine && mainFrame->GetSelectedMachine())
  {
    PopupMenu(mainFrame->menu_Unit, e.m_x, e.m_y+15);
    //PopupMenu(selectedMachine->GetMenu());
  } else {
    PopupMenu(mainFrame->menu_View, e.m_x, e.m_y+15);
  }

}

void CGLDisplay::OnMotion(wxMouseEvent &e)
{
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition)
  {
    //posSetX=x;
    //posSetY=y;
    float z;
    screenToGl(e.m_x,e.m_y,0, posSetX, posSetY, z);
  } else {
    //if dragging, move view
//     if (e.Dragging() && e.LeftIsDown())
//     {
//       view.OriginX-=float(e.m_x-mouseClickX)/(float)view.Scale;
//       view.OriginY+=float(e.m_y-mouseClickY)/(float)view.Scale;
//       mouseClickX=e.m_x;
//       mouseClickY=e.m_y;
//     }
    OnMouseMove(e.m_x, e.m_y);
  }
}

void CGLDisplay::OnMouseWheel(wxMouseEvent &e)
{
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition)
  {
    posSetHeading+=(1.0*e.GetWheelRotation()/e.GetWheelDelta())*0.05;
    dPrint(1,"setting position %f",posSetHeading);
  } else {
    //zoom
//     int oldScale=view.Scale;
    int dir=e.GetWheelRotation();
    int delta=e.GetWheelDelta();
    int newScale;
    if (delta!=0)
      newScale=view.Scale+view.Scale*dir/delta*0.10;
    
//   
//     if (view.Scale<SCALE_MIN)
//       view.Scale=SCALE_MIN;
//   
//     if (view.Scale>SCALE_MAX)
//       view.Scale=SCALE_MAX;
//     
     float glx, gly, glz;
     screenToGl(e.m_x, e.m_y, 0, glx, gly, glz);
     
     Zoom(glx,gly,newScale);
//     
//     //move view center so that the location under pointer remains on same spot on the screen
//     float ratio=(1.0-1.0*oldScale/view.Scale);
//     view.OriginX+=(glx-view.OriginX)*ratio;
//     view.OriginY+=(gly-view.OriginY)*ratio;
  }
}

void CGLDisplay::OnLeaveWindow(wxMouseEvent &e)
{
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition)
  {
    dPrint(1,"leaving, x=%d y=%d",e.m_x,e.m_y);
    int x=e.m_x;
    int y=e.m_y;
    
    if (x < 0) {
      x=0;
      view.OriginX-=3.0/view.Scale;
    }
    if (x >= this->GetSize().GetWidth()) {
      x=this->GetSize().GetWidth()-1;
      view.OriginX+=3.0/view.Scale;
    }
    if (y < 0) {
      y=0;
      view.OriginY+=3.0/view.Scale;
    }
    if (y >= this->GetSize().GetHeight()) {
      view.OriginY-=3.0/view.Scale;
      y=this->GetSize().GetHeight()-1;
    }
    WarpPointer(x, y);
  }
}

void CGLDisplay::OnChar(wxKeyEvent &e)
{
  switch (e.GetKeyCode())
  {
    case '+':
      ZoomIn();
      break;
    case '-':
      ZoomOut();
      break;
    default:
      e.Skip();
      break;
  }
}

void CGLDisplay::OnKeyDown(wxKeyEvent &e)
{
  switch (e.GetKeyCode())
  {
    case WXK_UP:
      keyUp=true;
      break;
    case WXK_DOWN:
      keyDown=true;
      break;
    case WXK_LEFT:
      keyLeft=true;
      break;
    case WXK_RIGHT:
      keyRight=true;
      break;
    case WXK_SHIFT:
      keyBoost=true;
      break;
    case WXK_TAB:
      mainFrame->SelectNextMachine(e.ShiftDown());
      break;
    case WXK_ESCAPE:
      if (mainFrame->GetSelectedMachine() && 
          mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition)
      {
        mainFrame->DeselectCurrent();
      }
      break;
    case WXK_SPACE:
      CenterToSelected();
    case WXK_CONTROL:
      editingPath=true;
      break;
    case WXK_DELETE:
      if (mainFrame->GetSelectedMachine() && 
          mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeCoordDrive)
      {
        mainFrame->GetSelectedMachine()->RemoveSelectedPathPoints();
      }
      break;
    case WXK_RETURN:
      if (mainFrame->GetSelectedMachine() && 
          mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeCoordDrive)
      {
        mainFrame->GetSelectedMachine()->StartDrivingPath();
      }
      break;
    default:
      e.Skip();
      break;
  }
  
}

void CGLDisplay::OnKeyUp(wxKeyEvent &e)
{
  switch (e.GetKeyCode())
  {
    case WXK_UP:
      keyUp=false;
      break;
    case WXK_DOWN:
      keyDown=false;
      break;
    case WXK_LEFT:
      keyLeft=false;
      break;
    case WXK_RIGHT:
      keyRight=false;
      break;
    case WXK_SHIFT:
      keyBoost=false;
      break;
    case WXK_CONTROL:
      editingPath=false;
      break;
    default:
      e.Skip();
      break;
  }

}


/************************************************************/
void CGLDisplay::handleArrowKeys()
{
  //dPrint(1,"handling keys");
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeManualDrive)
  {
    // in manual drive, arrows will drive robot
    if ((keyUp || keyDown || keyRight || keyLeft) && mainFrame->GetSelectedMachine())
    {
      //dPrint(1,"driving");
      float power=0;
      float steer=0;
      
      if (keyUp || keyDown)
        keyUp ? power=throttle : power=-throttle;
      
      if (keyBoost)
        power=power*2;
      
      if (power>1.0)
        power=1.0;
      
      if (keyRight || keyLeft)
        keyRight ? steer=-throttle : steer=throttle;
      
      if (keyBoost)
        steer=steer*2;
      
      if (mainFrame->GetSelectedMachine())
        mainFrame->GetSelectedMachine()->Drive(power,steer);
      
      driving=true;
    } else if (driving) {
      //when driving ends, stop the machine
      if (mainFrame->GetSelectedMachine())
        mainFrame->GetSelectedMachine()->Drive(0,0);
      driving=false;
    }
  } else {
    //otherwise arrow keys are used to move the view
    float x_dir=0;
    float y_dir=0;
      
    if (keyUp || keyDown)
      keyUp ? y_dir=1.0 : y_dir=-1.0;
      
    if (keyRight || keyLeft)
      keyRight ? x_dir=1.0 : x_dir=-1.0;
    
    view.OriginX=view.OriginX+x_dir/view.Scale*10.0;
    view.OriginY=view.OriginY+y_dir/view.Scale*10.0;
  }
  //dPrint(1,"handling keys done");
}

void CGLDisplay::setView()
{

  if (followSelected)
    CenterToSelected();
  
  view.Left=view.OriginX-this->GetSize().GetWidth()/(float)view.Scale/2.0;
  view.Right=view.OriginX+this->GetSize().GetWidth()/(float)view.Scale/2.0;

  view.Bottom=view.OriginY-this->GetSize().GetHeight()/(float)view.Scale/2.0;
  view.Top=view.OriginY+this->GetSize().GetHeight()/(float)view.Scale/2.0;

  SetCurrent(GLContext);
  
  glClearColor(CLEAR_COLOR,0);

  glEnable(GL_DEPTH_TEST);
  
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glViewport(0,0,GetSize().GetWidth(),GetSize().GetHeight());
  glOrtho(view.Left,view.Right,view.Bottom,view.Top,-1000,1000);

  double pm[16];
  glGetDoublev(GL_PROJECTION_MATRIX,pm);
  SetProjectionMatrix(pm);
  
}

void CGLDisplay::draw()
{
  SetCurrent(GLContext);
  
  setView();
  
  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
  
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  
  double mm[16];
  glGetDoublev(GL_MODELVIEW_MATRIX,mm);
  SetModelviewMatrix(mm);
  
  if (grid)
    drawGrid();
  
  drawMachines();
  
  if (mainFrame->GetSelectedMachine() && 
      mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition)
  {
    mainFrame->GetSelectedMachine()->DrawGhost(posSetX,posSetY,posSetHeading);
  }
  
  drawHUD();
  
  glFlush();
  SwapBuffers();
}

void CGLDisplay::drawGrid()
{
  float x, y;
  glLineWidth(1);
  glBegin(GL_LINES);
    //draw grid
  glColor3f(GRID_COLOR);
  for (x= view.Left-fmod(view.Left,gridSize); x<=view.Right;x+=gridSize)
  {
    glVertex3f(x,view.Top,gridZ);
    glVertex3f(x,view.Bottom,gridZ);
  }
  for ( y=view.Bottom-fmod(view.Bottom,gridSize); y<=view.Top; y+=gridSize)
  {
    glVertex3f(view.Left,y,gridZ);
    glVertex3f(view.Right,y,gridZ);
  }
    //draw coordinate axes
  glColor3f(AXIS_COLOR);
  glVertex3f(0,view.Top,gridZ+0.1);
  glVertex3f(0,view.Bottom,gridZ+0.1);
  glVertex3f(view.Left,0,gridZ+0.1);
  glVertex3f(view.Right,0,gridZ+0.1);
  glEnd();
}

void CGLDisplay::drawMachines()
{
  //dPrint(1,"Drawing machines");
  std::map<std::string, CMachine*>::iterator i;
  for (i=mainFrame->machines.begin(); i!=mainFrame->machines.end(); i++)
  {
    (i->second)->SetViewParams(view);
    (i->second)->Draw();
  }
      
}

void CGLDisplay::drawHUD()
{
  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  
  gluOrtho2D(0,view.Width,0,view.Height);
  
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();
  
  glTranslatef(20,20,0);
  glColor3f(1,1,0);
  if (mainFrame->GetSelectedMachine())
  {
    drawText(0,20,0,GLUT_BITMAP_HELVETICA_10, mainFrame->GetSelectedMachine()->GetName().c_str());
    if (mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeCoordDrive)
    {
      drawText(0,0,0,GLUT_BITMAP_HELVETICA_10, "Coordinate drive");
    } else if (mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeManualDrive){
      drawText(0,0,0,GLUT_BITMAP_HELVETICA_10, "Manual drive");
    } else if (mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeSetPosition){
      drawText(0,0,0,GLUT_BITMAP_HELVETICA_10, "Setting position");
    } else if (mainFrame->GetSelectedMachine()->GetOperationMode()==KOperModeObservation){
      drawText(0,0,0,GLUT_BITMAP_HELVETICA_10, "Observation mode");
    }
  }
  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();
  glMatrixMode(GL_PROJECTION);
  glPopMatrix();
  glPopAttrib();
}

/*****************************************************************/

int CGLDisplay::ThreadFunction(const int aThreadNumber)
{
  //ownSleep_ms(1000); //this prevents a segfault :P
  //dPrint(1,"thread running");
  while (!IsRequestTermination())
  {
    handleArrowKeys();
    
    //refresh graphics
    wxPaintEvent e;
    AddPendingEvent(e);
    
    ownSleep_ms(50);
  }
  return 0;
}

const TViewParams& CGLDisplay::GetViewParams()
{
  return view;
}


void CGLDisplay::OnPick(TGLPickEvent event)
{
  if (event.Pickable!=this)
  {
    //dPrint(1,"picked");
    picked=true;
    if (event.Button)//==1 || event.Button==3)
    {
      CMachine* m=(CMachine*)(event.Pickable);
      mainFrame->SelectMachine(m->GetName());
      //dPrint(1,"machine %s picked",m->GetName().c_str());
      if (event.Button==3) {
        CenterToSelected();
      } else if (event.Button==2) {
        contextMenuOnMachine=true;
      }
    }
  } else {
    //dPrint(1,"drag plane picked");
  }
}

void CGLDisplay::OnDrag(TGLPickEvent event)
{
  if (event.Pickable==this)
  {
    //dPrint(1,"draggin");
    view.OriginX-=float(event.ScreenX-mouseClickX)/(float)view.Scale;
    view.OriginY+=float(event.ScreenY-mouseClickY)/(float)view.Scale;
    mouseClickX=event.ScreenX;
    mouseClickY=event.ScreenY;
  }
}

void CGLDisplay::DrawPickable()
{
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  glBegin(GL_QUADS);
  glVertex3f(view.Left, view.Top,-1000);
  glVertex3f(view.Right, view.Top,-1000);
  glVertex3f(view.Right, view.Bottom,-1000);
  glVertex3f(view.Left, view.Bottom,-1000);
  glEnd();
  
}

void CGLDisplay::CenterToSelected()
{
  if (mainFrame->GetSelectedMachine())
  {
    view.OriginX=mainFrame->GetSelectedMachine()->GetPositionX();
    view.OriginY=mainFrame->GetSelectedMachine()->GetPositionY();
  } else {
    view.OriginX=0;
    view.OriginY=0;
  }
}

void CGLDisplay::Zoom(float x, float y, int scale)
{
  int oldScale=view.Scale;
  
  view.Scale=scale;
  
  if (scale<SCALE_MIN)
    view.Scale=SCALE_MIN;
  
  if (scale>SCALE_MAX)
    view.Scale=SCALE_MAX;
  
  //move view center so that the location under pointer remains on same spot on the screen
  float ratio=(1.0-1.0*oldScale/view.Scale);
  view.OriginX+=(x-view.OriginX)*ratio;
  view.OriginY+=(y-view.OriginY)*ratio;
}

void CGLDisplay::ZoomIn()
{
  Zoom(view.OriginX,view.OriginY,view.Scale+view.Scale*0.10);
}

void CGLDisplay::ZoomOut()
{
  Zoom(view.OriginX,view.OriginY,view.Scale-view.Scale*0.10);
}

/*
void CGLDisplay::SetOperationMode(EOperationMode mode)
{
  switch (mode)
  {
    case KOperModeSetPosition:
      if (mainFrame->GetSelectedMachine() && mainFrame->GetSelectedMachine()->HasPosition())
      {
        operationMode=KOperModeSetPosition;
        posSetX=mainFrame->GetSelectedMachine()->GetPositionX();
        posSetY=mainFrame->GetSelectedMachine()->GetPositionY();
        posSetHeading=mainFrame->GetSelectedMachine()->GetHeading();
      }
      break;
    case KOperModeCoordDrive:
      operationMode=KOperModeCoordDrive;
      editingPath=true;
      break;
    default:
      break;
  }
}*/
